/*************************************************************
 * @file motor.c
 * @author MicroOrange
 * @brief 
 * @date 2023/2/18
 *************************************************************/

#include "crc16.h"
#include "aim_484.h"
#include "math.h"
#include "stdlib.h"

void aim_motor_init(motor_aim *motor,UART_HandleTypeDef *uart, uint8_t dev_addr){
    motor->uart = uart;
    motor->dev_addr = dev_addr;
}

bool aim_write_reg(motor_aim *motor, uint8_t addr, uint16_t val){
    uint16_t crc_temp;
    motor->tx_buffer[0] = motor->dev_addr;
    motor->tx_buffer[1] = AIM_COMMAND_WRITE_REG;
    motor->tx_buffer[2] = 0x00;
    motor->tx_buffer[3] = addr;
    motor->tx_buffer[4] = (val >> 8)&0xFF;
    motor->tx_buffer[5] = val&0xFF;
    crc_temp = crc16(motor->tx_buffer, 6);
    motor->tx_buffer[6] = (crc_temp >> 8)&0xFF;
    motor->tx_buffer[7] = crc_temp&0xFF;

    HAL_UART_Transmit(motor->uart, motor->tx_buffer, 8, MOTOR_UART_TIMEOUT);
    HAL_UART_Receive(motor->uart, motor->rx_buffer, 8, MOTOR_UART_TIMEOUT);
    delay_us(100);
    if (memcpy(motor->tx_buffer, motor->rx_buffer, 8) == 0)
        return true;
    else
        return false;

}

uint16_t aim_read_reg(motor_aim *motor, uint8_t addr){
    uint16_t crc_temp, ret=0;
    motor->tx_buffer[0] = motor->dev_addr;
    motor->tx_buffer[1] = AIM_COMMAND_READ_REG;
    motor->tx_buffer[2] = 0x00;
    motor->tx_buffer[3] = addr;
    motor->tx_buffer[4] = 0x00;
    motor->tx_buffer[5] = 0x01;
    crc_temp = crc16(motor->tx_buffer, 6);
    motor->tx_buffer[6] = (crc_temp>>8)&0xFF;
    motor->tx_buffer[7] = crc_temp&0xFF;
    HAL_UART_Transmit(motor->uart, motor->tx_buffer, 8, MOTOR_UART_TIMEOUT);
    HAL_UART_Receive(motor->uart, motor->rx_buffer, 8, MOTOR_UART_TIMEOUT);
    ret |= motor->tx_buffer[4];
    ret |= (motor->tx_buffer[3] << 8U);
    delay_us(100);
    return ret;
}

void aim_write_pu(motor_aim *motor, uint32_t val, uint8_t addr){
    uint8_t pu24_31, pu16_23, pu8_15, pu0_7;
    uint16_t crc_temp;
    if (val < 0){
        val = 0x7FFFFFFF + val + 1;
        pu24_31 = trunc((double)val / (256.0 + 65535.0)) + 0x80;
    } else{
        pu24_31 = trunc((double)val / (256.0 + 65535.0));
    }

    pu16_23 = val >> 16;
    pu8_15 = val >> 8;
    pu0_7 = val & 0xFF;

    motor->tx_buffer[0] = motor->dev_addr;
    motor->tx_buffer[1] = AIM_COMMAND_WRITE_PU;
    motor->tx_buffer[2] = 0x00;
    motor->tx_buffer[3] = addr;
    motor->tx_buffer[4] = 0x00;
    motor->tx_buffer[5] = 0x02;
    motor->tx_buffer[6] = 0x04;
    motor->tx_buffer[7] = pu8_15;
    motor->tx_buffer[8] = pu0_7;
    motor->tx_buffer[9] = pu24_31;
    motor->tx_buffer[10] = pu16_23;
    crc_temp = crc16(motor->tx_buffer, 11);
    motor->tx_buffer[11] = (crc_temp >> 8)&0xFF;
    motor->tx_buffer[12] = crc_temp&0xFF;
    HAL_UART_Transmit(motor->uart, motor->tx_buffer, 13, MOTOR_UART_TIMEOUT);
    HAL_UART_Receive(motor->uart, motor->rx_buffer, 8, MOTOR_UART_TIMEOUT);
}

